#include "MREAgent.h"

#include <rlglue/utils/C/RLStruct_util.h>
#include <rlglue/utils/C/TaskSpec_Parser.h>
#include <string.h>
#include "MDPSolver.h"
using namespace planning; 


//this is only to delay step function in simulation to see the effect of threading. otherwise steps will run too fast to let planning complete
#ifdef USE_THREADS
#ifdef OS_WINDOWS
#include <windows.h>
#else
#include <unistd.h>
#define Sleep(t)  usleep(t)
#endif
#endif

 


//----------------- --------RL Glue interface ----------------------------------

MREAgent* agent; 
observation_t	globalObservation; 
action_t		globalAction; 


void agent_init(const char* task_spec)
{
	agent = new MREAgent(); 

	taskspec_t ts; 
	if (decode_taskspec(&ts, task_spec) != 0)
	{
		printf("Error: cannot parse the task spec\n"); 
		exit(1); 
	}

	allocateRLStruct(&globalAction, ts.num_int_actions, ts.num_double_actions, ts.charcount_actions); 
	allocateRLStruct(&globalObservation, ts.num_int_observations, ts.num_double_observations, ts.charcount_observations); 

	free_taskspec_struct(&ts); 


	agent->init(task_spec); 
}

const action_t *agent_start(const observation_t *this_observation) 
{
	replaceRLStruct(this_observation, &globalObservation); 
	Action a =  agent->start(globalObservation.doubleArray); 
	globalAction.intArray[0] = a; 
	return &globalAction; 
}

const action_t *agent_step(double reward, const observation_t *this_observation) 
{
#ifdef USE_THREADS
//		Sleep(100); 
#endif

	replaceRLStruct(this_observation, &globalObservation); 
	Action a = agent->step(reward, globalObservation.doubleArray ); 
	globalAction.intArray[0] = a; 
	return &globalAction; 
}

void agent_end(double reward) 
{
	agent->end(reward); 
}

void agent_cleanup() 
{
//	freeRLStructPointer(&globalObservation); 
//	freeRLStructPointer(&globalAction); 
	clearRLStruct(&globalObservation); 
	clearRLStruct(&globalAction); 
	agent->cleanup(); 

	delete agent; 
}

const char* agent_message(const char* inMessage) {
	printf("message received: '%s'\n", inMessage); 
		return agent->message(inMessage); 
}

// -----------------------end RL Glue interface ------------------------------

void logValueFunction()
{
	char fname [20] = {0}; 
	sprintf(fname, "data/vf-%d.out", agent->stats.trialAge); 

	FILE* fout = fopen(fname, "w"); 

	int resolution = 30; //param: how fine do want to discretize when sending to monitor
	int rows = resolution * resolution; 
	int cols = 3; //this is always 3 (x,y and value)

	int result = 0; 

	//make an observation
	Observation o = new Observation_type[MREAgent::dimension]; 
	

	for (int i=0; i< resolution; i++)
	{
		o[0] =	agent->taskSpec.double_observations[0].min  + i*(agent->taskSpec.double_observations[0].max  - agent->taskSpec.double_observations[0].min )/(resolution -1); 
		if (o[0] == agent->taskSpec.double_observations[0].max  ) 
			o[0] -= 0.001; 
		else if (o[0] == agent->taskSpec.double_observations[0].min)
			o[0] += 0.001; 

		for(int j=0; j< resolution; j++)
		{
			o[1] =	agent->taskSpec.double_observations[1].min + j*(agent->taskSpec.double_observations[1].max - agent->taskSpec.double_observations[1].min)/(resolution -1); 
			if (o[1] == agent->taskSpec.double_observations[1].max ) 
				o[1] -= 0.0001; 
			else if (o[1] == agent->taskSpec.double_observations[1].min)
				o[1] += 0.0001; 

			
			double ss = 0; 
			double tmpDouble = 0; 
			Observation tmpObs=0; 
			ss = agent->mdpSolver->getStateValue(o); 
			
			fprintf(fout, "%3.3lf\t%3.3lf\t%3.3lf\n", o[0], o[1], ss); 
		}
	}

	delete[] o; 
	fclose(fout); 
}

#ifndef OS_WINDOWS
/*void sendNodeDimension(RL_socket& conn, KNode* n)

if (!n || !n->box)
		return;

	conn.send(n->box->mins[0],1);
	conn.send(n->box->maxs[0],1);
	conn.send(n->box->mins[1], 1);
	conn.send(n->box->maxs[1],1);
	double knownness = n->getKnownness(); 
	conn.send(knownness, 1); 

	if (n->left)
		sendNodeDimension(conn, n->left);
	
	if (n->right)
		sendNodeDimension(conn, n->right);


}

void sendScatteredPoints(RL_socket& conn)
{
	int s=1; 
	conn.send(s,1);
	int mdim = agent->dimension; 
	if (mdim > 2)
		mdim = 2; 

	int size= agent->history.size(); 

	conn.send(size,1); 
	conn.send(mdim,1); 
		
	for(list<Transition>::iterator it= agent->history.begin(); it != agent->history.end(); it++)
	{
			for(int j=0; j< mdim ; j++)
				conn.send( (*it).start[j], 1);
	}
	
}

void sendGridInfo(RL_socket& conn)
{
	int result =0; 
	int rows = 5;		//
	int cols =0; 

//	KNode * root = ((FittedVISolver*)agent->mdpSolver)->tree->root; 
	KNode * root = ((KDTreeLearner*) agent->tfGens)->learners[0][0].root; 

	cols = root->getNumberOfNodes(); 

	int s=1;
	result += conn.send(s, 1); //respond to value function
	result +=conn.send(cols, 1); 
	result +=conn.send(rows, 1); 
	sendNodeDimension(conn, root); 
}



void sendInfo(RL_socket& conn, int infoType, double param=0)
{
	printf("\n\nsendinfo of %d, %lf\n", infoType, param);

	int resolution = 30; //param: how fine do want to discretize when sending to monitor
	int rows = resolution * resolution; 
	int cols = 3; //this is always 3 (x,y and value)

	int result = 0; 

	if (infoType==3)
	{
		sendGridInfo(conn);
		return;
	}else if (infoType==4)
	{
		sendScatteredPoints(conn);
		return;
	}

	int s = 1; //MONITOR_VALUE_FUNCTION; 
	result += conn.send(s, 1); //respond to value function
	result +=conn.send(rows, 1); 
	result +=conn.send(cols, 1); 

	//make an observation
	Observation o = new Observation_type[MREAgent::dimension]; 
	
	for(int i=2; i< agent->dimension; i++)
		o[i] =  0; 

	for (int i=0; i< resolution; i++)
	{
		o[0] =	agent->taskSpec.obs_mins[0] + i*(agent->taskSpec.obs_maxs[0] - agent->taskSpec.obs_mins[0])/(resolution -1); 
		if (o[0] == agent->taskSpec.obs_maxs[0] ) 
			o[0] -= 0.001; 
		else if (o[0] == agent->taskSpec.obs_mins[0])
			o[0] += 0.001; 

		for(int j=0; j< resolution; j++)
		{
			o[1] =	agent->taskSpec.obs_mins[1] + j*(agent->taskSpec.obs_maxs[1] - agent->taskSpec.obs_mins[1])/(resolution -1); 
			if (o[1] == agent->taskSpec.obs_maxs[1] ) 
				o[1] -= 0.0001; 
			else if (o[1] == agent->taskSpec.obs_mins[1])
				o[1] += 0.0001; 

			
			double ss = 0; 
			double tmpDouble = 0; 
			Observation tmpObs=0; 
			switch(infoType)
			{
			case 1:		//send value function
				ss = agent->mdpSolver->getStateValue(o); 
//				ss = ( (FittedVISolver*) agent->mdpSolver)->getStateValue(o); 
				break; 
			case 2:		//send policy
				ss = agent->mdpSolver->getBestAction(o); 
				break; 
				//3 is reserved for grid info (handled seperately)
			case 4:		//sending scattered points
				sendScatteredPoints(conn); 
				break; 
			case 3:		//sending grid info (handled seperately)			
				break;
			case 5:
				tmpObs = agent->tfGens->predict(o,0); 
				if (tmpObs)
				{
					ss = tmpObs[0]- o[0]; 
					delete tmpObs; 
				}else
					ss = -1; 

				break; 
			case 6:
				tmpObs = agent->tfGens->predict(o,0); 
				if (tmpObs)
				{
					ss = tmpObs[1]- o[1]; 
					delete tmpObs; 
				}else
					ss = -1; 

				break; 

			case 7:
				tmpObs = agent->tfGens->predict(o,0); 
				if (tmpObs)
				{
					ss = tmpObs[2]- o[2]; 
					delete tmpObs; 
				}else
					ss = -1; 

				break; 

			}
			 
			result +=conn.send(o[0],1); 
			result +=conn.send(o[1],1); 
			result +=conn.send(ss, 1); 
		}
		
	}
	delete[] o; 
	printf("total of %d bytest sent \n", result); 
}
*/




//-----------------------end global function 

#endif
